#include <cmath>
#include "WPILib.h"
#include "Robot2489.h"

/** 
 * Written by Nate
 * 
 * double MotorControl(double jpos)
 * Takes a double between -1.0 and 1.0 as the joystick position
 * and returns a double between -1.0 and 1.0 as the resulting
 * intended motor speed.
 * 
 * I used the logistic growth function because I wanted to save my sanity.
 * All it does is multiply and divide by a few values and exponentiates
 * the mathematical constant E.
 * 
 * Notice that it does require the cmath library for the E exponentiation.
 * I assume it's possible to use this library with the robot.
 */

double Robot2489::MotorControl(double jpos)
{
	double motorspeed;	// define return value variable
	if(jpos >= -0.06 && jpos <= 0.06)	// define dead zone
		return 0.0;
	jpos = jpos * 10;	// Move decimal point. I don't know why; I spent a
						// half an hour messing with various different values,
						// it had to be like this.
	if(jpos >= 0)
	{
		motorspeed = 10/(1+250*std::exp(-jpos));	// function for positive values
		motorspeed = motorspeed/10;
	}
	else
	{
		motorspeed = -10/(1+250*std::exp(jpos));	// function for negative values
		motorspeed = motorspeed/10;
	}
	if(motorspeed >= 1.0)	// make sure return value doesn't exceed 1.0
		motorspeed = 1.0;
	return motorspeed;
}

void Robot2489::DriveForward(float speed, float time)
{
	myRobot->SetLeftRightMotorSpeeds(speed,speed);
	Wait(time);
	myRobot->SetLeftRightMotorSpeeds(0,0);
}

void Robot2489::DriveBackward(float speed, float time)
{
	myRobot->SetLeftRightMotorSpeeds(-speed,-speed);
	Wait(time);
	myRobot->SetLeftRightMotorSpeeds(0,0);
}

void Robot2489::TurnRight()
{
		myRobot->SetLeftRightMotorSpeeds(0.8,0.5);
		Wait(1);
		myRobot->SetLeftRightMotorSpeeds(0,0);
}
